function [ke,pe,e] = calcEnergy(x,length,param)

inertia = param{7};
mass = param{8};
GRAVITY = param{9};
NBODIES = param{14};

q = x(1:NBODIES,1);
u = x(NBODIES+1:end,1);

ke = 0;
pe = 0;

% Location and velocity of the inboard handle
p1 = [0 0]'; v1 = [0 0]';
for i=1:NBODIES
    
    % Transformation matrix between body and newtonian frame
    n_c_k         = [cos(sum(q(1:i))) -sin(sum(q(1:i)));
                    sin(sum(q(1:i))) cos(sum(q(1:i)))];
    
    % Location of center of mass of each body in body frame
    p2 = n_c_k*[length(i)/2 0]';
    cm = p1 + p2;
    
    p3 = p1 + 2*p2;
    p1 = p3;    
    
    % potential energy of each body.
    pe(i) = mass(i)*9.81*cm(2,1);
    
    % Absolute Angular velocity of each body
    w = sum(u(1:i));
    
    % Velocity of center of mass
    v2 = v1 + w*[-p2(2) p2(1)]';
    
    % Kinetic energy
    ke(i) = 0.5*(mass(i)*(v2(1)^2+v2(2)^2) + inertia(3,3,i)*w^2);            
    
    v3 = v1 + w*2*[-p2(2) p2(1)]';
    v1 = v3;
end

e = sum(ke)+sum(pe);